/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/helpSrc/tape_half_auto.cpp
 * @Description  : 实现了半自动导航功能。
 *                 在半自动模式下，AGV可以在手动和自动操作之间切换，并根据磁条信息进行路径跟踪和纠偏。
 *                 代码包含了模式切换、速度设定、纠偏算法以及输入信号检测等功能。
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:56:09
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "tape_half_auto.h" 
#include "motion_parameters.h"
#include "public_kernel.h"
constexpr int RUN_PERIOD = 50;              // 运行周期-50ms
constexpr int OCCUR_TIME = 3000;            // 4s
constexpr float MAX_DEAD_DISTANCE = 0.3f;   // 最大允许盲走距离
constexpr float FOLLOW_TAPE_MAX_VEL = 1.0f; // 巡线最大速度
constexpr float MAX_SLOPE_VEL = 0.5f;       // 最大加减速度
constexpr const char PARAM_NAV_TYPE[] = "/AGVParam/agv_navigation_type";
constexpr const char CMD_VEL_TOPIC[] = "cmd_vel"; //"/base/twist_mux/keyboard_vel"
// 打舵、事件停止、触发逻辑
TapeHalfAuto::TapeHalfAuto() 
{
    initialized_ = false;
    current_stage_ = HalfRunStage::Stage_Manual;
    Reset();
}

TapeHalfAuto::~TapeHalfAuto() 
{
    quit_mutex_.unlock();
    thread_.join();
}

void TapeHalfAuto::CycleRoutine() 
{
    std::lock_guard<std::mutex> locker(data_mutex_);
    // #1 从半自动跳走，则复位一次模式
    AgvWorkMode mode = GetCurMode();
    if (mode != last_mode_) {
        if (last_mode_ == AgvWorkMode::mod_semiauto) {
            if (current_stage_ != HalfRunStage::Stage_Manual) {
                DEBUG_WARN_OUT("force set stageManulReady:m_curStage=" << current_stage_);
                current_stage_ = HalfRunStage::Stage_Manual;
            }
        }
        DEBUG_WARN_OUT("change op mode once m_lastMode=" << static_cast<int>(last_mode_) << ",newMode=" << static_cast<int>(mode));
        last_mode_ = mode;
    }

    ////////////////////////////////#1 手柄速度控制触发巡线/////////////////////////////////////
    bool manual_vel_mode = run_param_.use_mode == UseMode::ManualVelMode;
    auto node = ROSNode();
    if (manual_vel_mode) {
        switch (current_stage_) {
            case HalfRunStage::Stage_Manual: {
                Reset();
                ReadParam();
                ChangeNavMode(MAG_GUIDANCE);
                current_stage_ = HalfRunStage::Stage_HalfNormalRun;
                DEBUG_OUT("stageManulToHalfAuto finish: Stage_Manual -> Stage_HalfNormalRun");
            } break;

            case HalfRunStage::Stage_HalfAgain: {
                DEBUG_OUT_THROTTLE(node->get_logger(), 
                    *(node->get_clock()), 1000.0, "Stage_HalfAgain wait reset....");
            } break;

            case HalfRunStage::Stage_HalfNormalRun: {
                float vel = 0;
                if (joy_vel_.IsNew(500)) {
                    auto data = joy_vel_.Get();
                    vel = data.linear.x;
                } else {
                    DEBUG_OUT_THROTTLE(node->get_logger(), 
                            *(node->get_clock()), 1000.0, "joy vel time out,set vel is 0");
                }
                // 磁条消失，则报错处理
                bool failed = FinishStatus::Error_Finish == StageNormalHalfRun(vel);
                if (failed) {
                    current_stage_ = Stage_HalfAgain;
                    DEBUG_OUT("stageNormalHalfRun Error_Finish: Stage_HalfNormalRun -> Stage_HalfAgain");
                }
            } break;

            default:
                break;
        }
        DEBUG_OUT_THROTTLE(node->get_logger(), 
                *(node->get_clock()), 1000.0, "manualVelMode: TapeHalfAuto cycle:m_curStage=" << current_stage_ << ",mode=" << static_cast<int>(mode) << ",m_lastMode=" << static_cast<int>(last_mode_));
        return;
    }

    //////////////////////////////#2 按钮触发自动巡线模式////////////////////////////////////
    // #2 当前是自动模式，不执行逻辑
    bool exit_btn = NeedForceFinish();
    if (exit_btn && mode == AgvWorkMode::mod_semiauto) {
        DEBUG_WARN_OUT("exitBtn to manual mode");
        StageFinishToManual();
    }
    if (exit_btn || mode == AgvWorkMode::mod_auto_online || mode == AgvWorkMode::mod_auto_offline) {
        if (current_stage_ != HalfRunStage::Stage_Manual) {
            vel_.SetZero();
            SetVel(vel_);
            DEBUG_WARN_OUT("autoMode or push button to exit;runModeMisMatch:m_curStage=" << current_stage_ << ",exitBtn=" << exit_btn << ",mode=" << static_cast<int>(mode));
            current_stage_ = HalfRunStage::Stage_Manual;
        }
        return;
    }

    switch (current_stage_) {
        case HalfRunStage::Stage_Manual: {
            if (StageManualToHalfAuto()) {
                Reset();
                ReadParam();
                ChangeNavMode(MAG_GUIDANCE);
                current_stage_ = HalfRunStage::Stage_HalfNormalRun;
                DEBUG_OUT("stageManualToHalfAuto finish: Stage_Manual -> Stage_HalfNormalRun");
            }
        } break;

        case HalfRunStage::Stage_HalfAgain: {
            if (StageHalfReadyAgain()) {
                Reset();
                ReadParam();
                ChangeNavMode(MAG_GUIDANCE);
                current_stage_ = Stage_HalfNormalRun;
                DEBUG_OUT("stageHalfReadyAgain finish: Stage_HalfAgain -> Stage_HalfNormalRun");
            }
        } break;

        case HalfRunStage::Stage_HalfNormalRun: {
            switch (StageNormalHalfRun(run_param_.auto_vel)) {
                case FinishStatus::Normal_Finish: {
                    current_stage_ = Stage_HalfFinish;
                    DEBUG_OUT("stageNormalHalfRun Normal_Finish : Stage_HalfNormalRun -> Stage_HalfFinish");
                } break;

                case FinishStatus::Force_Finish: {
                    current_stage_ = Stage_ForceFinish;
                    DEBUG_OUT("stageNormalHalfRun  Force_Finish: Stage_HalfNormalRun -> Stage_ForceFinish");
                } break;

                case FinishStatus::Error_Finish: {
                    current_stage_ = Stage_HalfAgain;
                    DEBUG_OUT("stageNormalHalfRun Error_Finish: Stage_HalfNormalRun -> Stage_HalfAgain");
                } break;

                default:
                    break;
            }
        } break;

        case HalfRunStage::Stage_ForceFinish: {
            if (StageFinishToManual()) {
                current_stage_ = Stage_Manual;
                DEBUG_OUT("stageFinishToManual finish: Stage_ForceFinish -> Stage_Manual");
            }
        } break;

        case HalfRunStage::Stage_HalfFinish: {
            if (StageFinishToAuto()) {
                current_stage_ = Stage_Manual;
                DEBUG_OUT("OK;stageFinishToAuto finish: Stage_HalfFinish -> Stage_Manual");
            }
        } break;

        default:
            break;
    }
    
    DEBUG_OUT_THROTTLE(node->get_logger(), 
                *(node->get_clock()), 1000.0, "normal TapeHalfAuto cycle:m_curStage=" << 
        current_stage_ << ",mode=" << static_cast<int>(mode) << ",m_lastMode=" << static_cast<int>(last_mode_));
}

void TapeHalfAuto::ThreadFunc() 
{
    while (!quit_mutex_.try_lock_for(std::chrono::milliseconds(RUN_PERIOD))) {
        CycleRoutine();
    }
}

bool TapeHalfAuto::Init() 
{
    if (initialized_) {
        DEBUG_OUT("TapeHalfAuto init finish before ");
        return true;
    }

    last_mode_ = GetCurMode();
    InitIo();
    ReadParam();

    vel_pub_ = ROSNode()->create_publisher<geometry_msgs::msg::Twist>(CMD_VEL_TOPIC, 1); // 半自动下也发自动模式话题
    sub_io_new_ = ROSNode()->create_subscription<agv_msgs::msg::ReadInPutsNew>(
        "/readInputNew", 3, 
        std::bind(&TapeHalfAuto::ReadInputNewCallBack, this, std::placeholders::_1));

    sub_magnet_data_ = ROSNode()->create_subscription<agv_msgs::msg::MagnetInfo>(
        "/magnetDataInfo", 4, 
        std::bind(&TapeHalfAuto::GetMagnetDataCB, this, std::placeholders::_1)); // 订阅 landmark
    
    sub_joy_vel_ = ROSNode()->create_subscription<geometry_msgs::msg::Twist>(
        "/semi_auto_vel", 1, 
        std::bind(&TapeHalfAuto::GetJoyVelCB, this, std::placeholders::_1));     // 订阅手柄速度

    op_mode_client_ = ROSNode()->create_client<agv_srvs::srv::SetOperationMode>("/operation_mode");
    quit_mutex_.lock();
    thread_ = std::thread(std::bind(&TapeHalfAuto::ThreadFunc, this));
    DEBUG_OUT("TapeHalfAuto init finish first ");
    initialized_ = true;

    while (!op_mode_client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        DEBUG_ERROR_OUT("Interrupted while waiting for the service. Exiting.");
        return false;
      }
      DEBUG_OUT("Operation_mode service not available, waiting again...");
    }

    return true;
}

void TapeHalfAuto::Reset() 
{
    vel_.SetZero();
    modify_ctrl_.Clear();
    current_dead_size_ = 0.0f;
    signal_stage_ = SIGNAL_VALID;
    ClearHalfAutoPidCtrl(0);
}

void TapeHalfAuto::InitIo() 
{
    // 初始化io key-名字
    agv_msgs::msg::PairTypeInt io;
    io.value = 0;

    // 急停按钮
    io.key = EMG_BUTTON;
    emg_btn_.Update(io);

    // 停止按钮
    io.key = STOP_BUTTON;
    stop_btn_.Update(io);

    // 放行按钮
    io.key = RELEASE_BUTTON;
    release_btn_.Update(io);

    // 磁地标信号
    io.key = MAG_MARK_IO;
    mag_detect_io_.Update(io);
}

void TapeHalfAuto::SetVel(const Velocity& vel) 
{
    AgvWorkMode mode = GetCurMode();
    if (mode == AgvWorkMode::mod_semiauto) {
        geometry_msgs::msg::Twist cmd;
        cmd.linear.x = vel.vel_x_;
        cmd.linear.y = vel.vel_y_;
        cmd.angular.z = vel.angular_;
        vel_pub_->publish(cmd);
        DEBUG_OUT("half auto setVel:vx=" << vel.vel_x_ << ",vy=" << vel.vel_y_ << ",w=" << vel.angular_);
    }
}

void TapeHalfAuto::GetMagnetDataCB(const agv_msgs::msg::MagnetInfo& data) {
    std::lock_guard<std::mutex> locker(data_mutex_);
    magnet_info_.Update(data);
}

void TapeHalfAuto::GetJoyVelCB(const geometry_msgs::msg::Twist& data) {
    joy_vel_.Update(data);
    auto joyVel = joy_vel_.Get();
    
    DEBUG_OUT_THROTTLE(ROSNode()->get_logger(), 
            *(ROSNode()->get_clock()), 500, "joy vel:vx=" << joyVel.linear.x << 
            ",vy=" << joyVel.linear.y << ",w=" << joyVel.angular.z);
}

// 产生导航纠偏量，AMR只产生角速度;全向车产生vx vy w三个反向的矢量
ModifyCtrl TapeHalfAuto::GenAngularVel(float vel) {
    auto data = magnet_info_.Get();
    signal_stage_ = (magnet_info_.IsNew(100) && data.valid) ? SIGNAL_VALID : SIGNAL_INVALID;

    if (!magnet_info_.IsNew(100)) {
        DEBUG_WARN_OUT("halfAuto magInfo time out");
    }

    switch (signal_stage_) {
        case SIGNAL_VALID: {
            // 清除当前盲走距离
            current_dead_size_ = 0.0f;

            // AMR 纠偏系数
            float a1 = 0.0f, a2 = 0.0f;

            // #2 find revise param1 param2
            FindLevelParam(vel, a1, a2);

            // #3-1 当前的控制周期(50ms)
            float period = RUN_PERIOD / 1000.0f;

            // #3-2 角速度纠偏加速度：低速时加速度8°/s^2 ;高速加速度 6°/s^2
            // 角速度斜坡值
            const float angle_vel_step = fabs(vel) > 0.5 ? period * TO_RADIAN(6) : period * TO_RADIAN(8);

            // #3-3 vx vy 方向纠偏加速度： 低速时加速度0.15m/s^2 ;高速加速度 0.1m/s^2
            // 速度斜坡值
            const float vel_step = fabs(vel) > 0.5 ? period * 0.1 : period * 0.15;

            // 如果是倒退;y方向偏差给反
            data.offset_y = SIGN(vel) * data.offset_y;
            ModifyCtrl ctrl_data = FollowPathForHalfAuto(data.offset_y, data.angle, vel, a1, a2);

            if (ctrl_data.CanIgnore()) {
                modify_ctrl_.Clear();
                DEBUG_OUT("FollowPath can ignore; force clear m_modifyCtrl");
            }

            Approach(modify_ctrl_.delta_velocity_x, ctrl_data.delta_velocity_x, vel_step);
            Approach(modify_ctrl_.delta_velocity_y, ctrl_data.delta_velocity_y, vel_step);
            Approach(modify_ctrl_.angular_velocity, ctrl_data.angular_velocity, angle_vel_step);
        } break;

        case SIGNAL_INVALID: {
            modify_ctrl_.Clear();
            ClearHalfAutoPidCtrl(0);
            // 计算实际的累加长度
            float dead_dis = IntegralDis(GetRealVel());
            if (dead_dis > MAX_DEAD_DISTANCE) {
                DEBUG_ERROR_OUT("magsignal invalid:fDeadDis=" << dead_dis);
            }
        } break;
    }
    return modify_ctrl_;
}

void TapeHalfAuto::NormalOutVel(Velocity& control_vel, ModifyCtrl& modify_ctrl) {
    // 最大纠偏分量上限值0.1m/s
    const float max_delta_x = 0.1, max_delta_y = 0.1;
    float radius = MotionParm::min_adjust_radius_;
    radius = radius<= 0.00001f ? 1.0f : radius;

    float low_divide = 3;
    float high_divide = 1.5;
    radius = fabs(control_vel.linear_) < 0.5f ? (radius / low_divide) : (radius / high_divide);

    // 根据当前的规划速度 限制当前纠偏量
    LimitModifyV(control_vel, modify_ctrl);

    // 如果此时线速度为0，角速度纠偏则也为0(防止原地转圈纠偏)
    float max_delta_w = fabs(control_vel.linear_ / radius);
    modify_ctrl.angular_velocity = Limit(modify_ctrl.angular_velocity, max_delta_w);
    modify_ctrl.delta_velocity_x = Limit(modify_ctrl.delta_velocity_x, max_delta_x);
    modify_ctrl.delta_velocity_y = Limit(modify_ctrl.delta_velocity_y, max_delta_y);

    DEBUG_OUT("normalizationVel out modifyCtrl:w=" << modify_ctrl.angular_velocity << 
            ",vx=" << modify_ctrl.delta_velocity_x << ",vy=" << modify_ctrl.delta_velocity_y << 
            ",maxW=" << max_delta_w << ",maxVx=" << max_delta_x << ",maxVy=" << max_delta_y);

    control_vel.angular_ += modify_ctrl.angular_velocity;
    control_vel.vel_x_ += modify_ctrl.delta_velocity_x;
    control_vel.vel_y_ += modify_ctrl.delta_velocity_y;
}

// 全方位车型纠偏限制：限制x y 方向的速度(防止舵角打舵过大)
void TapeHalfAuto::LimitModifyV(const Velocity& plan_vel, ModifyCtrl& modify) {
    // #1 限制y方向纠偏量
    const float max_steer_angle = TO_RADIAN(15); // 20度
    float delta_vy = modify.delta_velocity_y;
    float angle_y = 0.0f;

    if (fabs(plan_vel.vel_x_) > 0.001) {
        angle_y = atan(delta_vy / plan_vel.vel_x_); //[-pi/2,pi/2]
    }
    angle_y = Limit(angle_y, max_steer_angle); // 限制纠偏角度最大20度[-20,20]
    modify.delta_velocity_y = tan(angle_y) * plan_vel.vel_x_;

    // #2 限制x方向纠偏量
    float delta_vx = modify.delta_velocity_x;
    float angle_x = 0.0;

    if (fabs(plan_vel.vel_y_) > 0.001) {
        angle_x = atan(delta_vx / plan_vel.vel_y_); //[-pi/2,pi/2]
    }
    angle_x = Limit(angle_x, max_steer_angle); // 限制纠偏角度最大20度[-20,20]
    modify.delta_velocity_x = tan(angle_x) * plan_vel.vel_y_;

    DEBUG_OUT("TapeHalfAuto: LimitModifyV:delta_vy(old)=" << delta_vy << ",plan_vel.vel_x_=" <<
             plan_vel.vel_x_ << ",angle_y=" << angle_y << ",delta_vy(new)=" << modify.delta_velocity_y << 
             ",delta_vx(old)=" << delta_vx << ",plan_vel.vel_y_=" << plan_vel.vel_y_ << ",angle_x=" << 
             angle_x << ",delta_vx(new)=" << modify.delta_velocity_x);
}

void TapeHalfAuto::FindLevelParam(float v, float& a1, float& a2) {
    ParamSubType dir_param = ParamSubType::LaserLineForward;
    for (int i = 0; i < 10; i++) {
        bool next_index = fabsf(v) > MotionParm::NormalVel(i);
        if (next_index) {
            if (9 == i) {
                a1 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_y_;
                a2 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_angle_;
                break;
            }
            continue;
        } else {
            a1 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_y_;
            a2 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_angle_;
            break;
        }
    }
}

// 计算理论和实际的积分长度
float TapeHalfAuto::IntegralDis(float vel) {
    const float min = 0.04f; // 0.04m/s 速度 以内不参与里程计计算
    if (fabs(vel) < min) {
        DEBUG_OUT(" half auto fRealVel can ignore; so set vel is zero;fRealVel=" << vel);
        vel = 0.0f;
    }
    current_dead_size_ += vel * RUN_PERIOD / 1000.0f;
    DEBUG_OUT(" halfAuto: accumulatedDis:fRealVel=" << vel << ",fcurRealSize=" << current_dead_size_);
    return current_dead_size_;
}


// 默认500ms以内都是有效
float TapeHalfAuto::GetRealVel(float timeout)
{
    float velocity = 0.0f;
    float time_diff_ms = 0.0f;
    const agv_msgs::msg::OdometryLite &odom = GetOdomData();
    time_diff_ms = (ROSTime() - odom.header.stamp).seconds() * 1000.0;

    if (time_diff_ms < timeout) {
        float fx = odom.twist.linear.x;
        float fy = odom.twist.linear.y;
        velocity = sqrt(Square(fx) + Square(fy));
    } else {
        DEBUG_ERROR_OUT("Error: getRealVel: odom data is timeout: time_diff_ms=" << time_diff_ms
                        << ", timeout=" << timeout);
    }
    return velocity;
}

bool TapeHalfAuto::SwitchMode(const std::string mode)
{
    auto request = std::make_shared<agv_srvs::srv::SetOperationMode::Request>();
    request->mode = mode;
    request->sender = MODE_REQUEST::SENDER_SOURCE_NORMAL;
    request->submode = MODE_REQUEST::SUB_MODE_MASTER_AUTO;

    using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::SetOperationMode>::SharedFuture;
    auto response_received_callback = [](ServiceResponseFuture future) {
        DEBUG_OUT("msg=" << future.get()->message << ", ret=" << future.get()->success);    
    };

    auto result = op_mode_client_->async_send_request(request, response_received_callback);
  
    if (result.valid()) {
        DEBUG_OUT("Request successfully sent");
        return true;
    } else {
        DEBUG_OUT("Failed to send request");
        return false;
    }
}

// 检测输入IO信号是否触发
// 返回：是否触发
bool TapeHalfAuto::InputIo(INPUT_IO_TYPE& input_io)
{
    bool ret = false;
    // 如果数据有效，判断是否触发
    bool timeout = input_io.TimeOut(300);
    bool pressed = input_io.Get().value;

    if (timeout) {
        DEBUG_WARN_OUT("InputIo: data timeout: key=" << input_io.Get().key);
        return ret;
    }
    ret = pressed;
    return ret;
}

// 新更新输入IO对象
void TapeHalfAuto::UpdateInputIoNew(const agv_msgs::msg::ReadInPutsNew& msg, INPUT_IO_TYPE& io)
{
    auto input_io = io.Get();
    std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg.array;

    for (auto &data : inputs_msg) {
        if (data.key == input_io.key) {
            io.Update(data);
            break;
        }
    }
}

// 获取IO输入信号回调函数 io topic话题频率30hz
void TapeHalfAuto::ReadInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg)
{
    // 并更新数据的时间戳
    UpdateInputIoNew(msg, stop_btn_);
    UpdateInputIoNew(msg, release_btn_);
    UpdateInputIoNew(msg, emg_btn_);
    UpdateInputIoNew(msg, mag_detect_io_);
    LogMark(mag_detect_io_);
}

// 地标日志打印
void TapeHalfAuto::LogMark(INPUT_IO_TYPE &mark)
{
    // 打印磁地标
    static DataTimeStamp<bool> timed_mark(mark.Get().value);

    // 获取当前地标状态
    auto data = mark.Get();
    if (data.value) {
        DEBUG_OUT("logMark_true: key=" << data.key << ", value=" << data.value);
        timed_mark.Update(data.value);
    } else if (timed_mark.IsNew(3000)) {
        DEBUG_OUT("logMark_false: key=" << data.key << ", value=" << data.value);
    }
}

bool TapeHalfAuto::StageManualToHalfAuto()
{
    // 静态定时器；0:复位状态 1：开始触发计时
    static DataTimeStamp<bool> press_timer(false);
    // 上一次触发过,延迟关闭
    static DataTimeStamp<bool> last_occur_once(false);
    // 当前工作模式
    AgvWorkMode mode = GetCurMode();

    // 未按下按钮-不触发
    bool pressed = InputIo(release_btn_);
    if (!pressed) {
        // 触发后等1s，内模式跳转了，则认为时最新的跳转操作(操作模式反馈的滞后性)
        if (last_occur_once.IsNew(1000) && last_occur_once.Get() == true) {
            if (mode == AgvWorkMode::mod_semiauto) {
                last_occur_once.Update(false);
                DEBUG_WARN_OUT("stageManulToHalfAuto OK: last_occur_once is delayed 1s jump to half auto, last_occur_once set false");
                return true;
            }
        }
        press_timer.Update(false);
        return false;
    }

    // 查看是否复位过，防止第一次时间戳可能会误触发
    bool can_start  = press_timer.Get();
    if (!can_start) {
        DEBUG_WARN_OUT("stageManulToHalfAuto: start waiting to be pressed for 4s...");
        press_timer.Update(true);
        last_occur_once.Update(false);
        return false;
    }

    // 不断检测是否触发定时器-长按4s
    bool time_occur = press_timer.TimeOut(OCCUR_TIME);
    if (time_occur) {
#ifdef AGV_DEBUG_NAV
        return true;
#endif
        bool ok = SwitchMode(MODE_REQUEST::MODE_HALF_AUTO);
        if (ok) {
            last_occur_once.Update(true);
            DEBUG_OUT("stageManulToHalfAuto: time occurred, last_occur_once is true; switchMode call srv ok=" << ok);
        }
        if (mode == AgvWorkMode::mod_semiauto) {
            last_occur_once.Update(false);
            DEBUG_OUT("stageManulToHalfAuto: time occurred, stageManulToHalfAuto OK, current mode is already semiauto, last_occur_once is false");
            press_timer.Update(false);
            return true;
        }
    } else {
        DEBUG_WARN_OUT("stageManulToHalfAuto: waiting to be pressed for 4s...");
    }
    return false;
}

bool TapeHalfAuto::StageHalfReadyAgain()
{
    static DataTimeStamp<bool> press_timer(false);
    // 当前工作模式
    AgvWorkMode mode = GetCurMode();

    // 自动模式-无法切换-不触发
    if (mode != AgvWorkMode::mod_semiauto) {
        press_timer.Update(false);
        DEBUG_ERROR_OUT("stageHalfReadyAgain: mode is not half auto");
        return false;
    }

    // 未按下按钮-不触发
    bool pressed = InputIo(release_btn_);
    if (!pressed) {
        press_timer.Update(false);
        return false;
    }

    // 查看是否复位过，防止第一次时间戳可能会误触发
    bool can_start = press_timer.Get();
    if (!can_start) {
        DEBUG_WARN_OUT("stageHalfReadyAgain: start wait to pressed to 4s...");
        press_timer.Update(true);
        return false;
    }

    // 不断检测是否触发定时器-长按4s
    bool time_occur = press_timer.TimeOut(OCCUR_TIME);
    if (time_occur) {
        // 清除一下时间戳
        press_timer.Update(true);
        DEBUG_OUT("stageHalfReadyAgain: time occur");
        return true;
    } else {
        DEBUG_WARN_OUT("stageHalfReadyAgain: wait to pressed to 4s...");
    }
    return false;
}

bool TapeHalfAuto::NeedStop()
{
    bool ret = false;
    uint8_t control_mode = EventOccur();
    bool emg_stop = control_mode == agv_msgs::msg::AGVEvent::ESTOP;
    bool normal_stop = control_mode == agv_msgs::msg::AGVEvent::STOP;

    if (emg_stop || normal_stop) {
        DEBUG_OUT("needInterrupt eventOccur stop=" << emg_stop << ", normal_stop=" << normal_stop);
        ret = true;
    }
    return ret;
}

bool TapeHalfAuto::NeedForceFinish()
{
    bool ret = false;
    bool stop = InputIo(stop_btn_);
    bool emg = InputIo(emg_btn_);

    if (stop || emg) {
        DEBUG_OUT("needForceFinish stop=" << stop << ", emg=" << emg);
        ret = true;
    }
    return ret;
}

bool TapeHalfAuto::ErrorFinish()
{
    // 计算实际的累加长度
    if (current_dead_size_ > MAX_DEAD_DISTANCE) {
        DEBUG_ERROR_OUT("magsignal invalid: current dead size=" << current_dead_size_);
        return true;
    }
    return false;
}

bool TapeHalfAuto::NormalFinish()
{
    // 模式无效，则不会自动报完成
    bool valid = run_param_.use_mode == UseMode::BtnMode;
    if (!valid) {
        DEBUG_OUT_THROTTLE(ROSNode()->get_logger(), 
                *(ROSNode()->get_clock()), 1000.0f, "current is manual velocity mode");
        return false;
    }

    if (static_cast<int>(StopType::Type_InitStatus) == run_param_.stop_type) {
        auto data = magnet_info_.Get();
        bool is_new = magnet_info_.IsNew(100);
        bool valid = is_new && data.valid;

        if (!valid) {
            return false;
        }

        if (data.initstatus) {
            DEBUG_OUT("half auto normalFinish initstatus=" << static_cast<int>(data.initstatus));
        }
        return data.initstatus;
    } else if (StopType::Type_Mark == run_param_.stop_type) {
        bool mark = InputIo(mag_detect_io_);
        DEBUG_OUT("half auto normalFinish mark=" << mark);
        return mark;
    } else {
        DEBUG_ERROR_OUT("unknown m_run_param.stop_type=" << run_param_.stop_type);
    }
    return false;
}

bool TapeHalfAuto::RunModeMisMatch()
{
    // 运行模式非手动和非半自动
    AgvWorkMode mode = GetCurMode();
    return (mode != AgvWorkMode::mod_manual && mode != AgvWorkMode::mod_semiauto);
}

int TapeHalfAuto::StageNormalHalfRun(float target_v)
{
    // 当前工作模式
    AgvWorkMode mode = GetCurMode();

    // 自动模式-无法切换-不触发
    if (mode != AgvWorkMode::mod_semiauto) {
        DEBUG_ERROR_OUT("stageNormalHalfRun: mode is not half auto");
    }

    // #1 打舵逻辑简化版本
    //  如果正在打舵或者误差超限
    //  尝试打舵
    bool auto_btn_mode = run_param_.use_mode == UseMode::BtnMode;
    if (auto_btn_mode) {
        if (OtherStopSignal()) {
            vel_.SetZero();
            DEBUG_OUT("autoBtnMode: steering ...");
            // 不断打舵
            if (!Steering(target_v)) {
                return FinishStatus::Not_Finish;
            }
        }
    }

    if (std::abs(target_v) > FOLLOW_TAPE_MAX_VEL) {
        DEBUG_ERROR_OUT("stageNormalHalfRun: run target_v is too big; target_v=" << target_v);
        target_v = target_v > 0.00001f ? FOLLOW_TAPE_MAX_VEL : -FOLLOW_TAPE_MAX_VEL;
    }

    // #2 给定速度-如果需要加减速-设置规划
    if (!NeedStop()) {
        Approach(vel_.vel_x_, target_v, MAX_SLOPE_VEL * (RUN_PERIOD / 1000.0f));
    } else {
        Approach(vel_.vel_x_, 0.0f, 2 * MAX_SLOPE_VEL * (RUN_PERIOD / 1000.0f));
    }

    // 规划 vel 和 w 值为 0
    vel_.vel_y_ = 0.0f;
    vel_.angular_ = 0.0f;

    // 合速度
    vel_.linear_ = vel_.vel_x_;

    // 产生纠偏
    ModifyCtrl modify_ctrl = GenAngularVel(vel_.linear_);

    // 限制纠偏，叠加输出 vel_
    NormalOutVel(vel_, modify_ctrl);

    // 模式不是自动上线模式，不会触发
    if (NormalFinish()) {
        vel_.SetZero();
        SetVel(vel_);
        DEBUG_WARN_OUT("stop stageNormalHalfRun: normalFinish OK");
        return FinishStatus::Normal_Finish;
    }

    if (ErrorFinish()) {
        vel_.SetZero();
        SetVel(vel_);
        DEBUG_WARN_OUT("stop stageNormalHalfRun: errorFinish OK");
        return FinishStatus::Error_Finish;
    }

    SetVel(vel_);
    return FinishStatus::Not_Finish;
}

bool TapeHalfAuto::StageFinishToManual()
{
    // 获取当前模式
    AgvWorkMode mode = GetCurMode();
    bool ok = SwitchMode(MODE_REQUEST::MODE_MANUL);
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    if (ok) {
        DEBUG_OUT("stageFinishToManul switchMode manual finish");
    }

    if (mode == AgvWorkMode::mod_manual) {
        DEBUG_OUT("stageFinishToManul finish: mode is manual");
        return true;
    }
    return false;
}

bool TapeHalfAuto::StageFinishToAuto()
{
    // 获取当前模式
    AgvWorkMode mode = GetCurMode();
    bool ok = SwitchMode(MODE_REQUEST::MODE_AUTO);
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    if (ok) {
        DEBUG_OUT("stageFinishToAuto switchMode auto finish");
    }

    if (mode == AgvWorkMode::mod_auto_online) {
        DEBUG_OUT("stageFinishToAuto finish: mode is auto");
        return true;
    }
    return false;
}

// 原地起步打舵逻辑 true:完成 false:准备就绪
// 发送打舵指令和输出速度指令
bool TapeHalfAuto::Steering(float target_v)
{
    bool ret = false;

    if (!HasSteer()) {
        ret = true;
        DEBUG_OUT("steering is finished: AGV is not omni");
        return ret;
    }

    Velocity vel;
    vel.SetZero();
    // 舵角指令正负
    vel.vel_x_ = target_v > 0.001f ? 0.0001f : -0.0001f;
    DoSteerCmd(vel, static_cast<int>(SteerDirReport::SteerDirAuto), false);
    SetVel(vel);

    if (FinishSteerCmd()) {
        ret = true;
        DEBUG_WARN_OUT("TapeHalfAuto OK; FinishSteerCmd: set steering false");
    }
    return ret;
}

void TapeHalfAuto::ReadParam()
{
    const std::string param_base = "tape_half_auto.";
    run_param_.auto_vel = ROSNode()->get_parameter_or(param_base + "vel", 0.2f);
    run_param_.stop_type = ROSNode()->get_parameter_or(param_base + "stop_type", static_cast<int>(StopType::Type_InitStatus));
    run_param_.use_mode = ROSNode()->get_parameter_or(param_base + "use_mode", static_cast<int>(UseMode::BtnMode));
    DEBUG_OUT("TapeHalfAuto readParam: vel=" << run_param_.auto_vel << ", stop_type=" << run_param_.stop_type
            << ", use_mode=" << run_param_.use_mode);
}

void TapeHalfAuto::ChangeNavMode(USHORT mode)
{
    rclcpp::Parameter param(PARAM_NAV_TYPE, mode);
    if(ROSNode()->set_parameter(param).successful) {
        DEBUG_OUT("Set param: changeNavMode " << mode << " success.");
    } else {
        DEBUG_OUT("Set param: changeNavMode " << mode << " failed.");
    }
}
